// --------------------------------------------------------------------------------------------------------------------
// <copyright file="URControlAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.Runtime.UniversalRobots
{
    using System;
    using System.Diagnostics;
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Core.Algorithms;
    using Microsoft.Robotics.Hardware.UniversalRobots;
    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Manipulation.JointController;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Manipulation.Runtime.MotionControl;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;

    /// <summary>
    /// Control agent for UR arm
    /// </summary>
    [DataContract]
    [System.Diagnostics.CodeAnalysis.SuppressMessage(
        "Microsoft.Design",
        "CA1001:TypesThatOwnDisposableFieldsShouldBeDisposable",
        Justification = "Agent handles disposal of unmanaged resources in OnDeactivate()")]
    public class URControlAgent :
ConsumerProducerAgent<TimerMessage, ManipulatorStatusMessage>,
        IControlReceiver<CartesianGoalPositionControlMessage>,
        IControlReceiver<LinearCartesianMotionControlMessage>,
        IControlReceiver<JointGoalPositionControlMessage>,
        IControlReceiver<SetJointVelocityPIDGainsMessage>
    {
        /// <summary>
        /// Number of ticks (uSec) after which Pid history is considered invalid.
        /// </summary>
        private const long PidControlExpiryTicks = 100000;

        /// <summary>
        /// Time after which two successive position commands are not considered to be part of the same spline
        /// </summary>
        private const double AdjacentPointTimeoutInMilliseconds = 50;

        /// <summary>
        /// Hostname for URControl server
        /// </summary>
        [DataMember(Name = "Hostname")]
        private string hostname;

        /// <summary>
        /// Port URControl server is listening on
        /// </summary>
        [DataMember(Name = "ControlServerPortNumber")]
        private int urControlServerPort;

        /// <summary>
        /// Port URControl real time interface is on
        /// </summary>
        [DataMember(Name = "RealTimePortNumber")]
        private int urRealTimePort;

        /// <summary>
        /// P gain for controlling positions via velocity
        /// </summary>
        [DataMember(Name = "VelocityControlledJointPositionPGain")]
        private double[] pGains;

        /// <summary>
        /// D gain for controlling positions via velocity
        /// </summary>
        [DataMember(Name = "VelocityControlledJointPositionDGain")]
        private double[] dGains;

        /// <summary>
        /// Deadband for controlling positions via velocity
        /// </summary>
        [DataMember(Name = "VelocityControlledJointPositionDeadband")]
        private double[] deadband;

        /// <summary>
        /// Max delta for controlling positions via velocity
        /// </summary>
        [DataMember(Name = "VelocityControlledJointPositionMaxDelta")]
        private double[] maxDelta;

        /// <summary>
        /// Maximum joint velocity
        /// </summary>
        [DataMember(Name = "MaximumJointVelocity")]
        private double[] maxVelocity;

        /// <summary>
        /// Configuration data for UR arm
        /// </summary>
        private ConfigurationData configurationData = null;

        /// <summary>
        /// Controls joint position via velocity commands
        /// </summary>
        private VelocityControlledJointPositionController velocityControlledJointPositionController;

        /// <summary>
        /// Initializes a new instance of the <see cref="URControlAgent"/> class.
        /// </summary>
        /// <param name="name">Agent name.</param>
        /// <param name="producer">TimerMessage producer.</param>
        /// <param name="hostname">UR controller hostname.</param>
        /// <param name="controlServerPort">Control server port number.</param>
        /// <param name="realTimePort"> Real time port number</param>
        /// <param name="pGains">P gain for controlling positions via velocity</param>
        /// <param name="dGains">D gain for controlling positions via velocity</param>
        /// <param name="maxJointVelocity">Maximum joint velocity.</param>
        /// <param name="deadband"> Controller deadband. </param>
        /// <param name="maxDelta"> Max controller output delta between successive cycles. </param>
        public URControlAgent(
            string name,
            IProducer<TimerMessage> producer,
            string hostname,
            int controlServerPort,
        int realTimePort,
            double[] pGains,
            double[] dGains,
            double[] maxJointVelocity,
            double[] deadband,
            double[] maxDelta)
            : base(name, producer.Name)
        {
            this.SetHostAndPort(hostname, controlServerPort, realTimePort);
            this.SetGains(pGains, dGains, maxJointVelocity, deadband, maxDelta);
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="URControlAgent"/> class, using the provided 
        /// <c>IURControlClientNetworkConnection</c> instances instead of attempting to connect to 
        /// a UR controller.
        /// </summary>
        /// <param name="name">Agent name.</param>
        /// <param name="producer">TimerMessage producer.</param>
        /// <param name="urControlNetworkConnection">Network connection object for urControlClient.</param>
        /// <param name="urRealTimeNetworkConnection">Network connection object for real time port.</param>
        /// <param name="pGains">P gain for controlling positions via velocity</param>
        /// <param name="dGains">D gain for controlling positions via velocity</param>
        /// <param name="maxJointVelocity">Maximum joint velocity.</param>
        /// <param name="deadband"> Controller deadband. </param>
        /// <param name="maxDelta"> Max controller output delta between successive cycles. </param>
        internal URControlAgent(
            string name,
            IProducer<TimerMessage> producer,
            IURControlClientNetworkConnection urControlNetworkConnection,
            IURControlClientNetworkConnection urRealTimeNetworkConnection,
            double[] pGains,
            double[] dGains,
            double[] maxJointVelocity,
            double[] deadband,
            double[] maxDelta)
            : base(name, producer.Name)
        {
            // Network connections were provided, so these values are unused.
            this.SetHostAndPort(string.Empty, 0, 0);

            this.SetGains(pGains, dGains, maxJointVelocity, deadband, maxDelta);
            this.URControlNetworkConnection = urControlNetworkConnection;
            this.URRealTimeNetworkConnection = urRealTimeNetworkConnection;
        }

        /// <summary>
        /// Gets or sets the client for the URControl server
        /// </summary>
        internal URControlClient URControlClient { get; set; }

        /// <summary>
        /// Gets or sets the network connection for URControl server
        /// </summary>
        internal IURControlClientNetworkConnection URControlNetworkConnection { get; set; }

        /// <summary>
        /// Gets or sets the network connection for URControl server
        /// </summary>
        internal IURControlClientNetworkConnection URRealTimeNetworkConnection { get; set; }

        /// <summary>
        /// Initializes the agent
        /// </summary>
        /// <param name="locator">Agent locator to use</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);

            if (this.velocityControlledJointPositionController == null)
            {
                this.velocityControlledJointPositionController = this.BuildVelocityControlledJointPositionController();
            }
        }

        /// <summary>
        /// Called when the agent is about to start running
        /// </summary>
        public override void OnActivated()
        {
            if (this.URControlNetworkConnection == null)
            {
                this.URControlClient = new URControlClient(this.hostname, this.urControlServerPort, this.urRealTimePort);
            }
            else
            {
                this.URControlClient = new URControlClient(this.URControlNetworkConnection, this.URRealTimeNetworkConnection);
            }

            this.URControlClient.Connect();

            base.OnActivated();
        }

        /// <summary>
        /// Called when the agent is deactivated
        /// </summary>
        public override void OnDeactivated()
        {
            base.OnDeactivated();
            this.URControlClient.Dispose();
        }

        /// <summary>
        /// Receive heartbeat message and publish status
        /// </summary>
        /// <param name="timerMessage">Message to process</param>
        public override void Receive(TimerMessage timerMessage)
        {
            ManipulatorStatus status = this.URControlClient.ManipulatorStatus;

            if (status == null)
            {
                // arm not ready
                return;
            }

            if (status != null)
            {
                ManipulatorStatusMessage publishMsg = new ManipulatorStatusMessage(status, Stopwatch.GetTimestamp() * 1000 / Stopwatch.Frequency);

                this.Publisher.Post(publishMsg);
            }
        }

        /// <summary>
        /// Receive and handle linear arm Cartesian move control message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void ReceiveControl(JointGoalPositionControlMessage message)
        {
            if (this.configurationData == null)
            {
                try
                {
                    this.configurationData = this.URControlClient.ConfigurationData;
                }
                catch (InvalidOperationException)
                {
                    return;
                }
            }

            if (message.ArmJointPositions.Length != URControlClient.NumJoints)
            {
                throw new ArgumentException(
                    string.Format(
                    "UR-5 has {0} joints, but recieved JointGoalPositionControlMessage for {1} joints",
                     message.ArmJointPositions.Length,
                     URControlClient.NumJoints));
            }

            for (int i = 0; i < URControlClient.NumJoints; ++i)
            {
                if (message.ArmJointPositions[i] > this.configurationData.JointMaxLimit[i] || message.ArmJointPositions[i] < this.configurationData.JointMinLimit[i])
                {
                    throw new ArgumentException("Position command of: " + message.ArmJointPositions[i].ToString() + " for joint " + i.ToString() + " is out of range");
                }
            }

            // Velocity Controller Strategy
            this.velocityControlledJointPositionController.SetReferences(message.ArmJointPositions);

            if (this.URControlClient.JointController != this.velocityControlledJointPositionController)
            {
                this.URControlClient.JointController = this.velocityControlledJointPositionController;
            }
        }

        /// <summary>
        /// Receive and handle <c>SetJointVeolcityPIDGainsMessag</c> by creating a new velocity controller.
        /// </summary>
        /// <param name="message">Message to handle.</param>
        public void ReceiveControl(SetJointVelocityPIDGainsMessage message)
        {
            this.pGains[message.JointNumber] = message.P;
            this.dGains[message.JointNumber] = message.D;
            this.deadband[message.JointNumber] = message.Deadband;
            this.maxDelta[message.JointNumber] = message.MaxDelta;
            this.velocityControlledJointPositionController = this.BuildVelocityControlledJointPositionController();
        }

        /// <summary>
        /// Receive and handle arm Cartesian goal control message. CAUTION - joint configuration may be unpredictable.
        /// </summary>
        /// <param name="message">Message to process</param>
        public void ReceiveControl(CartesianGoalPositionControlMessage message)
        {
            this.GoToCartesianGoal(message.CartesianPosition);
        }

        /// <summary>
        /// Receive and handle arm Cartesian goal control message. CAUTION - joint configuration may be unpredictable.
        /// </summary>
        /// <param name="message">Message to process</param>
        public void ReceiveControl(LinearCartesianMotionControlMessage message)
        {
            this.GoToCartesianGoal(message.CartesianPosition);
        }

        /// <summary>
        /// Move to a Cartesian space goal.  CAUTION - joint configuration may be unpredictable.
        /// </summary>
        /// <param name="cartesianGoal">Cartesian goal.</param>
        private void GoToCartesianGoal(Pose cartesianGoal)
        {
            // need to use "Rotation Vector" notation defined by UR arm.
            Matrix4 ht = HomogenousTransform.CreateFromQuaternion(cartesianGoal.Orientation);
            AxisAngle aa = HomogenousTransform.ToAxisAngle(ht);

            Vector3 rotationVector = aa.Axis;
            rotationVector = rotationVector * aa.Angle;

            double[] cartesianGoalPosition = 
            {
                cartesianGoal.Position.X,
                cartesianGoal.Position.Y,
                cartesianGoal.Position.Z,
                rotationVector.X,
                rotationVector.Y,
                rotationVector.Z
            };

            this.URControlClient.JointController = null;
            this.URControlClient.SendCommand(URControlCommandBuilder.ConstructCartesianPositionGoalString(cartesianGoalPosition));
        }

        /// <summary>
        /// Set the hostname and port number.
        /// </summary>
        /// <param name="hostname">The hostname.</param>
        /// <param name="controlServerPort">Control server port number.</param>
        /// <param name="realTimePort"> Real time port number</param>
        private void SetHostAndPort(string hostname, int controlServerPort, int realTimePort)
        {
            this.hostname = hostname;
            this.urControlServerPort = controlServerPort;
            this.urRealTimePort = realTimePort;
        }

        /// <summary>
        /// Set PD gains for controlling position via velocity
        /// </summary>
        /// <param name="pGains">P gains.</param>
        /// <param name="dGains">D gains</param>
        /// <param name="maximumJointVelocity">Maximum joint velocity.</param>
        /// <param name="deadband"> Controller deadband. </param>
        /// <param name="maxDelta"> Controller output maximum delta for singe cycle. </param>
        private void SetGains(double[] pGains, double[] dGains, double[] maximumJointVelocity, double[] deadband, double[] maxDelta)
        {
            if (pGains.Length != URControlClient.NumJoints)
            {
                throw new ArgumentException(
                    string.Format(
                    "Number of elements in pGains: {0} does not match number of joints: {1}",
                    pGains.Length,
                    URControlClient.NumJoints));
            }

            if (dGains.Length != URControlClient.NumJoints)
            {
                throw new ArgumentException(
                    string.Format(
                    "Number of elements in dGains: {0} does not match number of joints: {1}",
                    dGains.Length,
                    URControlClient.NumJoints));
            }

            if (maximumJointVelocity.Length != URControlClient.NumJoints)
            {
                throw new ArgumentException(
                    string.Format(
                    "Number of elements in maximumJointVelocity: {0} does not match number of joints: {1}",
                    maximumJointVelocity.Length,
                    URControlClient.NumJoints));
            }

            if (deadband.Length != URControlClient.NumJoints)
            {
                throw new ArgumentException(
                    string.Format(
                    "Number of elements in deadband: {0} does not match number of joints: {1}",
                    deadband.Length,
                    URControlClient.NumJoints));
            }

            if (maxDelta.Length != URControlClient.NumJoints)
            {
                throw new ArgumentException(
                    string.Format(
                    "Number of elements in maxDelta: {0} does not match number of joints: {1}",
                    maxDelta.Length,
                    URControlClient.NumJoints));
            }

            this.pGains = pGains;
            this.dGains = dGains;
            this.maxVelocity = maximumJointVelocity;
            this.deadband = deadband;
            this.maxDelta = maxDelta;
        }

        /// <summary>
        /// Constructs the controller used to control position via velocity
        /// </summary>
        /// <returns>A controller</returns>
        private VelocityControlledJointPositionController BuildVelocityControlledJointPositionController()
        {
            PidController[] pidControllers = new PidController[URControlClient.NumJoints];

            for (int i = 0; i < pidControllers.Length; ++i)
            {
                pidControllers[i] = new PidController(
                       this.maxVelocity[i],
                       -this.maxVelocity[i],
                       0,
                       this.pGains[i],
                       0,
                       this.dGains[i],
                       PidControlExpiryTicks);
            }

            return new VelocityControlledJointPositionController(URControlClient.NumJoints, pidControllers, this.deadband, this.maxDelta, this.maxVelocity);
        }
    }
}
